#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<tf/transform_listener.h>
#include<geometry_msgs/PointStamped.h>
#include<geometry_msgs/TransformStamped.h>
#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include<geometry_msgs/Vector3Stamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include<sensor_msgs/Imu.h>
#include <uav/convert_accel.h>
#include<visualization_msgs/Marker.h>
#include<nav_msgs/Path.h>
#include<std_msgs/Float64.h>

//上升结束
int flag_pos=0;
//计时
double t;
//计数
int count_all=0;
int count_out=0;
//PID
double last_err_x=0;
double sum_err_x=0;
double last_err_y=0;
double sum_err_y=0;
double last_err_z=0;
double sum_err_z=0;
//全局变量
geometry_msgs::Vector3Stamped input_,output_,output_pid;
geometry_msgs::PoseStamped pose_out;
//储存计算得到的加速度
geometry_msgs::Vector3Stamped acc_cal;
geometry_msgs::TwistStamped last_twist;
std_msgs::Float64 set_p,acc_c;

//convert_.h函数

geometry_msgs::Vector3Stamped optimization(geometry_msgs::Vector3Stamped input,geometry_msgs::PoseStamped pose_,geometry_msgs::TwistStamped twst);
int testPos(geometry_msgs::PoseStamped pos);

//回调函数
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

geometry_msgs::PoseStamped current_pose;
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &msg1)
{
    current_pose=*msg1; 
}

geometry_msgs::TwistStamped current_twist;
void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &msg2)
{
    current_twist= *msg2;
    //ROS_INFO("x=%.2f",current_twist.twist.linear.x);
    //ROS_INFO("y=%.2f",current_twist.twist.linear.y);
    //ROS_INFO("z=%.2f",current_twist.twist.linear.z);
}


int main(int argc, char** argv)
{
    ros::init(argc,argv,"ecbf_node");
    ros::NodeHandle nh;

    //收发器
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Subscriber  current_vel=nh.subscribe<geometry_msgs::TwistStamped>
            ("/mavros/local_position/velocity_local",10,twist_cb);
    ros::Subscriber local_pos=nh.subscribe<geometry_msgs::PoseStamped>
            ("mavros/local_position/pose",10,&pose_cb);

    ros::Publisher marker_pub=nh.advertise<visualization_msgs::Marker>("marker",10);
    ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("trajectory", 10);
    ros::Publisher point_pub = nh.advertise<geometry_msgs::PointStamped>("point", 10);
    ros::Publisher pos_dst=nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local",10); 
    ros::Publisher accel_pub=nh.advertise<geometry_msgs::Vector3Stamped>
            ("/mavros/setpoint_accel/accel",10); 
    ros::Publisher err_value=nh.advertise<std_msgs::Float64>
            ("/errpub",10); 
    ros::Publisher acc_value=nh.advertise<std_msgs::Float64>
            ("/accpub",10); 

    
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //PID参数
    double p_x=1;
    double p_y=1;
    double p_z=1;
    double i_x=0;
    double i_y=0;
    double i_z=0;
    double d_x=0;
    double d_y=0;
    double d_z=0;

    input_.vector.x=0;
    input_.vector.y=0;
    input_.vector.z=1.2;

    //trajectory
    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    nav_msgs::Path path;
    path.header.stamp = current_time;
    path.header.frame_id = "map";


    visualization_msgs::Marker box;
    box.header.frame_id="map";
    box.lifetime=ros::Duration(0);
    box.action=visualization_msgs::Marker::ADD;
    box.ns="cube";
    box.pose.orientation.w=1.0;
    box.id=2;
    box.type=visualization_msgs::Marker::CUBE;
    box.scale.x=3;
    box.scale.y=3;
    box.scale.z=1;
    box.pose.position.x=0;
    box.pose.position.y=0;
    box.pose.position.z=1.5;
    box.color.r = 1.0;
    box.color.a = 0.85;

    output_pid.vector.x=0;
    output_pid.vector.y=0;
    output_pid.vector.z=0;
    
    //边缘
    visualization_msgs::Marker line_list;
    line_list.header.frame_id="map";
    line_list.lifetime=ros::Duration(0);
    line_list.ns="lines";
    line_list.action = visualization_msgs::Marker::ADD;
    line_list.pose.orientation.w = 1.0;
    line_list.id = 2;
    line_list.type = visualization_msgs::Marker::LINE_LIST;
    line_list.scale.x = 0.01;
    line_list.scale.y=0.01;
    line_list.scale.z=0.01;

    // Line list is blue
    line_list.color.b = 1.0;
    line_list.color.a = 1;
    geometry_msgs::Point p1;
    p1.x=-1.5;
    p1.y=-1.5;
    p1.z=1;
    geometry_msgs::Point p2;
    p2.x=1.5;
    p2.y=-1.5;
    p2.z=1;
    geometry_msgs::Point p3;
    p3.x=1.5;
    p3.y=1.5;
    p3.z=1;
    geometry_msgs::Point p4;
    p4.x=-1.5;
    p4.y=1.5;
    p4.z=1;
    geometry_msgs::Point p5;
    p5.x=-1.5;
    p5.y=-1.5;
    p5.z=2;
    geometry_msgs::Point p6;
    p6.x=1.5;
    p6.y=-1.5;
    p6.z=2;
    geometry_msgs::Point p7;
    p7.x=1.5;
    p7.y=1.5;
    p7.z=2;
    geometry_msgs::Point p8;
    p8.x=-1.5;
    p8.y=1.5;
    p8.z=2;

    line_list.points.push_back(p1);
    line_list.points.push_back(p2);
    line_list.points.push_back(p2);
    line_list.points.push_back(p3);
    line_list.points.push_back(p3);
    line_list.points.push_back(p4);
    line_list.points.push_back(p4);
    line_list.points.push_back(p1);

    line_list.points.push_back(p5);
    line_list.points.push_back(p6);
    line_list.points.push_back(p6);
    line_list.points.push_back(p7);
    line_list.points.push_back(p7);
    line_list.points.push_back(p8);
    line_list.points.push_back(p8);
    line_list.points.push_back(p5);

    line_list.points.push_back(p1);
    line_list.points.push_back(p5);
    line_list.points.push_back(p2);
    line_list.points.push_back(p6);
    line_list.points.push_back(p3);
    line_list.points.push_back(p7);
    line_list.points.push_back(p4);
    line_list.points.push_back(p8);

    ros::Rate rate(20.0);

    while(ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }
    
    set_p.data=input_.vector.z;
    acc_c=set_p;

    //位置控制
    pose_out.pose.position.x=0;
    pose_out.pose.position.y=0;
    pose_out.pose.position.z=1.5;

    for(int i=100;ros::ok()&&i>0;--i)
    {
        pos_dst.publish(pose_out);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    ros::Time last_request = ros::Time::now();

    while(ros::ok())
    {
        marker_pub.publish(line_list);
        line_list.header.stamp=ros::Time::now();
        marker_pub.publish(box);
        box.header.stamp=ros::Time::now();

        geometry_msgs::PointStamped this_point_stamped;
        this_point_stamped.header.stamp = current_time;
        this_point_stamped.header.frame_id = "map";
        this_point_stamped.point.x = current_pose.pose.position.x;
        this_point_stamped.point.y = current_pose.pose.position.y;
        this_point_stamped.point.z = current_pose.pose.position.z;

        point_pub.publish(this_point_stamped);

        if(current_state.mode!="OFFBOARD"&&
        (ros::Time::now()-last_request>ros::Duration(5.0)))
        {
            if(set_mode_client.call(offb_set_mode)&&
            offb_set_mode.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
            }
            last_request=ros::Time::now();
        }
        else
        {
            if(!current_state.armed&&
            (ros::Time::now()-last_request>ros::Duration(5.0)))
            {
                if(arming_client.call(arm_cmd)&&
                arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                }
                last_request=ros::Time::now();
            }
        }

        //spinOnce放在两次操作中
        if(flag_pos==0)
        {
            pos_dst.publish(pose_out);
            ros::spinOnce();
            rate.sleep();
            if(current_pose.pose.position.z>1.5)
            {
                last_twist=current_twist;
                t=ros::Time::now().toSec();
                flag_pos=1;
            }
        } 
        if(flag_pos==1)
        { 
            ROS_INFO("accel on");
            //pose发布
            geometry_msgs::PoseStamped this_pose_stamped;

            this_pose_stamped=current_pose;
        
            this_pose_stamped.header.stamp = current_time;
            this_pose_stamped.header.frame_id = "base";
            path.poses.push_back(this_pose_stamped);

            path_pub.publish(path);
        
            last_time = current_time;

            ros::spinOnce();
            rate.sleep();
            

            int flag=0;
            flag=testPos(current_pose);
            if(flag==3)
            {
                ROS_INFO("within the range");
            }
            if(flag==4)
            {
                ROS_INFO("out of range");
                count_out++;
            }
            count_all++;
            ROS_INFO("detected for %d times, out for %d times",count_all,count_out);
            //output_=optimization(input_,current_pose,current_twist);
            output_=input_;
            
            if(count_all>=2)
            {
                ROS_INFO("begin to record.");
                double dt=ros::Time::now().toSec()-t;
                t=ros::Time::now().toSec();
                acc_cal.vector.x=(current_twist.twist.linear.x-last_twist.twist.linear.x)/dt;
                acc_cal.vector.y=(current_twist.twist.linear.y-last_twist.twist.linear.y)/dt;
                acc_cal.vector.z=(current_twist.twist.linear.z-last_twist.twist.linear.z)/dt;
                last_twist=current_twist;
                ROS_INFO("acc_cal.z=%f",acc_cal.vector.z);
                std_msgs::Float64 tem1;
                tem1.data=acc_cal.vector.z;
                acc_value.publish(tem1);
 

                //加速度PID
                double err_x=output_.vector.x-acc_cal.vector.x;
                double err_y=output_.vector.y-acc_cal.vector.y;
                double err_z=output_.vector.z-acc_cal.vector.z;
                std_msgs::Float64 tem;
                tem.data=err_z;
                err_value.publish(tem);
                
                ROS_INFO("error_z=%f",err_z);

                sum_err_x+=err_x;
                sum_err_y+=err_y;
                sum_err_z+=err_z;
                if(count_all==2)
                {
                    accel_pub.publish(output_);
                }
                if(count_all>=3)
                {
                    ROS_INFO("PID control");
                    //目前加了积分环节
/*                     output_pid.vector.x+=p_x*err_x+i_x*sum_err_x+d_x*(err_x-last_err_x);
                    output_pid.vector.y+=p_y*err_y+i_y*sum_err_y+d_y*(err_y-last_err_y); 
                    output_pid.vector.z=p_z*err_z+i_z*sum_err_z+d_z*(err_z-last_err_z);*/
                    last_err_x=err_x;
                    last_err_y=err_y;
                    last_err_z=err_z;
                    ROS_INFO("after pid:%f",output_pid.vector.z); 
                    accel_pub.publish(output_pid);   
                }
                
                
            } 
                    
        } 
    }
    return 0;
}